#include "PathMap.h"
#include "Node.h"
#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Node.h"
#include <iostream>
#include <ctime>
#include <stdlib.h>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <utility>
#include <stack>
#include <queue>
#include <set>

#define GOALX 2.75
#define GOALY 0


using namespace std;


PathMap::PathMap(SDSimulator* sim, float gS, float mX, float mxX, float mY, float mxY){
    //initialize values
    simulator = sim;
    gridSize = gS;
    stepSizeMetric = 0.05;
    pathWidthMetric = 0.10;
    terrainWeight = 8;
    minX = mX;
    maxX = mxX;
    minY = mY;
    maxY = mxY;
    numXvalues = (int)floor((maxX - minX)/gridSize +.1); //extra .1 is for c++ being annoying and slightly 
    numYvalues = (int)floor((maxY - minY)/gridSize +.1); //underestimating division

    hmap = new float*[numXvalues];
    gmap = new float*[numXvalues];
    hmap[0] = new float[numXvalues*numYvalues];
    gmap[0] = new float[numXvalues*numYvalues];
	for (int i = 1; i < numXvalues; i++){
        hmap[i] = hmap[i-1]+numYvalues;
        gmap[i] = gmap[i-1]+numYvalues;
    }
    
    //code for calculating hmap
    for(int i = 0; i < numXvalues; i++){
       for(int j = 0; j < numYvalues; j++){
          hmap[i][j] = simulator->getPointHeight((minX + i*gridSize),(minY + j*gridSize));
          
       }
    }

    //code for calculating gmap
    for(int i = 0; i < numXvalues; i++){
       for(int j = 0; j < numYvalues; j++){
          gmap[i][j] = 0;
          for(int k = -1; k <=1; k++){
             for(int h = -1; h <=1; h++){
                int x = i + k; int y = j + h;
                float diff = 0;
                if(x >=0 && x < numXvalues && y >= 0 && y < numYvalues){ //if x and y are inbounds 
                    if(k == 0 || h == 0) //if point is only gridSize away
                        diff = (fabs(hmap[x][y] - hmap[i][j]))/gridSize;
                    else //if point is diagonal from (i,j) - therefore gridSize*sqrt(2) away
                        diff = (fabs(hmap[x][y] - hmap[i][j]))/(gridSize*sqrt(2));
                }
                if(diff > gmap[i][j]) 
                    gmap[i][j] = diff;
             }
          }
          
       }
    }

} 

PathMap::~PathMap(){
}

float PathMap::getHMapAt(float x, float y){
    int xInArray = (int)((x - minX)/gridSize);
    int yInArray = (int)((y - minY)/gridSize);
    return hmap[xInArray][yInArray];
}

float PathMap::getGMapAt(float x, float y){
    int xInArray = (int)((x - minX)/gridSize);
    int yInArray = (int)((y - minY)/gridSize);
    return gmap[xInArray][yInArray];
}

class smallest_f // comparison function for priority queue
{
public:
  bool operator() (PathMapNode* lhs, PathMapNode* rhs) const
  {
    return (lhs->f > rhs->f);
  }
};

class smallest_node // comparison function for multiset
{
public:
  bool operator() (PathMapNode* lhs, PathMapNode* rhs) const
  {
     int Xl = lhs->x;
     int Yl = lhs->y;
     int Xr = rhs->x;
     int Yr = rhs->y;
     bool answer = (Xl < Xr) || ((Xr == Xl) && (Yl < Yr));
     return answer;
  }
};

void PathMap::printGradMapToFile(const char*filename){
    ofstream fp_out;
    fp_out.open(filename, ios::out);
    for (int i=0; i<numXvalues; i++){
       for (int j=0; j<numYvalues; j++){
           fp_out << gmap[i][j] << " ";
           //fp_out << i+j << " ";
       }
       fp_out << endl;
    }
    fp_out.close();
}

void PathMap::printHeightMapToFile(const char*filename){
    ofstream fp_out;
    fp_out.open(filename, ios::out);
    for (int i=0; i<numXvalues; i++){
       for (int j=0; j<numYvalues; j++){
           fp_out << hmap[i][j] << " ";
       }
       fp_out << endl;
    }
    fp_out.close();
}

void PathMap::printPathToFile(const char*filename, vector<Point*> path){
    // create array
    float** pathmap;
    pathmap = new float*[numXvalues];
    pathmap[0] = new float[numXvalues*numYvalues];
	for (int i = 1; i < numXvalues; i++){
        pathmap[i] = pathmap[i-1]+numYvalues;
    }

    // copy hmap
    for(int i=0; i<numYvalues; i++){
       for(int j=0; j<numYvalues; j++){
           pathmap[i][j] = hmap[i][j];
       }
    }

    // draw path dots
    for (int i=0; i<(int)path.size(); i++){
        int xx = (int)floor((path[i]->x - minX)/gridSize);
        int yy = (int)floor((path[i]->y - minY)/gridSize);
        pathmap[xx][yy] = hmap[xx][yy]+0.2;
    }

    //output in a file
    ofstream fp_out;
    fp_out.open(filename, ios::out);
    for (int i=0; i<numXvalues; i++){
       for (int j=0; j<numYvalues; j++){
           fp_out << pathmap[i][j] << " ";
       }
       fp_out << endl;
    }
    fp_out.close();

}

// adds height and gradient to a zone around the Point, to avoid it in later paths.
void PathMap::addObstacle(Point* point){
   float obstacleWidthMetric = 0.1; //added obstacle will be 2*obstacleWidthMetric in width
   int obstacleWidth = floor(obstacleWidthMetric/gridSize + 0.5);
   int x = floor((point->x-minX)/gridSize +0.5);
   int y = floor((point->y-minY)/gridSize +0.5);
   for (int i=x-obstacleWidth; i<x+obstacleWidth; i++){
      for (int j=y-obstacleWidth; j<y+obstacleWidth; j++){
         if(i>-1 && i<numXvalues && j>-1 && j<numYvalues){
            gmap[i][j] = 50;
            hmap[i][j] = hmap[i][j]+0.2;//this is for visualisation, doesn't influence 2D path
         }
      }      
   }

}

// changes the weight assigned to the difficulty of the terrain in the cost function.
// Default value (in the constructor): 0.3
// Putting a higher weight will produce a path with less slopes/discontinuities.
// Putting a lower weight will produce a path that goes straighter towards the goal, with more slopes/discontinuities.
void PathMap::setTerrainWeight(float newWeight){
    // Ideal Values : 
    // for more straight path : 0.3
    // for more squiggly path : 1 (more than 1 doesn't make much of a difference)
    terrainWeight = newWeight;
}

//Helper function : compute a node's cost, which depends on the difficulty of the square of terrain
float PathMap::getNodeCost(PathMapNode* current, int pathWidth){
    //pathwidth is the half-size of the square in number of array cells.
    int x = current->x;
    int y = current->y;
 
    if ((x-pathWidth) < 0 || (x+pathWidth>numXvalues) || (y-pathWidth < 0) || (y+pathWidth>numYvalues)){
        //printf("Path Node is sticking out of the map!\n x=%d , y=%d, pathWidth=%d\n", x,y,pathWidth);
        return -1;
    }

    float cost = 0;
    float grad;
    for (int i=x-pathWidth; i<x+pathWidth; i++){
       for (int j=y-pathWidth; j<y+pathWidth; j++){
           grad = gmap[i][j]*gmap[i][j];
           cost += grad; 
       }
    }

    return terrainWeight*cost/(pathWidth*pathWidth*4);
}

//Helper function for findPath. returns NULL if the node is invalid or has any adjacent places with too high a gradient
//returns a new PathMapNode* otherwise
PathMapNode* PathMap::CreateFringeNode(PathMapNode* current, int i, int j, int moveSize){
    i = i*moveSize; // i = x dist of the step in the gmap grid to be taken
    j = j*moveSize; // j = y dist of the step in the gmap grid to be taken
    int PathWidth = (int)floor(pathWidthMetric/gridSize+0.5); //this is how many gmap array spaces should be checked ON EITHER SIDE of the dog (= size of the square)

    //first, check if this node is valid
    if(current->x + i < 0)
        return NULL;
    if(current->x + i >= numXvalues)
        return NULL;
    if(current->y + j < PathWidth + 5) //Dog cannot get too close to the edge of the sides, otherwise we are disqualified
        return NULL;
    if(current->y + j >= (numYvalues - (PathWidth + 5)))
        return NULL;

    //create the new node
  
    PathMapNode* newNode = new PathMapNode();
    newNode->x = current->x + i;
    newNode->y = current->y + j;
    newNode->parent = current;

    // cost g = lengh of the path (in number of steps) + accumulated difficulty of the path
    float cost = getNodeCost(current, PathWidth);
    if (cost<0){
        return NULL;// the square is partly off the map.
    }
    if (i!=0 && j!=0){
       //chessboard distance:
       newNode->g = current->g + sqrt(2) + cost;
    }else{
       newNode->g = current->g + 1 + cost;
    }

    // heuristic h = distance to goal (=cost of going straight to goal on flat ground)
    float deltax = fabs(current->x - floor((GOALX -minX)/gridSize+0.5));
    float deltay = fabs(current->y - floor((GOALY -minY)/gridSize+0.5));
    //chessboard distance heuristic: 
    newNode->h = sqrt(2)*min(deltax, deltay) + max(deltax, deltay) - min(deltax, deltay);
    newNode->f = newNode->g + newNode->h;

    return newNode;
}

vector<Point*> PathMap::findPath(){
    //Find start and finish points in the grid, corresponding to (GOALX, GOALY) in the simulator
    int startX = (int) ((0 - minX)/gridSize);
    int zeroY = (int)floor((0 - minY)/gridSize + .1);
    int finishY = (int)floor((GOALY - minY)/gridSize + .1);
    int finishX = (int)floor((GOALX - minX)/gridSize + .1);
    int moveSize = (int)floor(stepSizeMetric/gridSize + .1); //gridSize must be less or equal than .05 and an integer divisor of .05 (just use .005)
                                                   //the + .1 is for c++ being buggy and slightly misunderestimating the division
    printf("finishX = %d and moveSize = %d  numXvalues = %d  numYvalues = %d \n", finishX, moveSize, numXvalues, numYvalues);

    //Create start node
    PathMapNode* startNode = new PathMapNode();
    startNode->x = startX;
    startNode->y = zeroY;
    startNode->parent = NULL;
    float cost = getNodeCost(startNode, (int)floor(pathWidthMetric/gridSize+0.5));
    if (cost < 0){
       printf("Start node's square sticks out. Make the map bigger!\n");
       return convertNodeToPath(startNode);
    }
    startNode->g = cost;
    startNode->h = fabs(finishX - startX); //The min amount of moves to get to the finish
    startNode->f = startNode->h + startNode->g;

    
    //initialize priorityQueue and all the other stuff 
    priority_queue<PathMapNode*, vector<PathMapNode*>, smallest_f> Q;
    Q.push(startNode);
    multiset<PathMapNode*, smallest_node> expanded;
    multiset<PathMapNode*, smallest_node>::iterator it;

    //A* algorithm
    while(!Q.empty()){
        PathMapNode* current = Q.top(); //Obtain next candidate for expansion
        Q.pop();

        //check to make sure this node hasn't been expanded
        it=expanded.find(current);

        if(it==expanded.end()){ //If this node hasn't been expanded before

            expanded.insert(current); //mark as expanded node
            if(current->y == finishY && current->x == finishX) //if goal node
                return convertNodeToPath(current);

            for(int i = -1; i <= 1; i++){ // populate children
                for(int j = -1; j <= 1; j++){
                    if(i != 0 || j != 0){
                        PathMapNode* newNode = CreateFringeNode(current, i, j, moveSize); //get a new node corresponding to this move
                        if(newNode != NULL)  
                            Q.push(newNode);

                    }
                }
            }  
        }
    }
    return convertNodeToPath(startNode); //returns a path of size 1 if it cant find a path
}

//This function puts the path found into corresponding simulator points, and into a vector
vector<Point*> PathMap::convertNodeToPath(PathMapNode* goalNode){
    vector<Point*> toReturn;

    while(goalNode != NULL){
        Point* p = new Point();
        p->x = minX + gridSize*goalNode->x;
        p->y = minY + gridSize*goalNode->y;
        toReturn.push_back(p);

        goalNode = goalNode->parent;
    }

    return toReturn;
}

